/*
 * Copyright 2016 The Cartographer Authors
 *
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 *
 *      http://www.apache.org/licenses/LICENSE-2.0
 *
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#include "scan_matching/ceres_scan_matcher_2d.h"

#include <utility>
#include <vector>

#include "Eigen/Core"
#include "common/ceres_solver_options.h"
#include "mapping/grid_2d.h"
#include "scan_matching/occupied_space_cost_function_2d.h"

// add by zl 
#include "scan_matching/occupied_grid_cost_function_2d.h"

#include "scan_matching/rotation_delta_cost_functor_2d.h"
#include "scan_matching/translation_delta_cost_functor_2d.h"
#include "scan_matching/tsdf_match_cost_function_2d.h"
#include "transform/transform.h"
#include "ceres/ceres.h"
#include "glog/logging.h"

namespace cartographer {
namespace scan_matching {

CeresScanMatcher2D::CeresScanMatcher2D()
    : ceres_solver_options_(
          common::CreateCeresSolverOptions()) {
  ceres_solver_options_.linear_solver_type = ceres::DENSE_QR;
}

CeresScanMatcher2D::~CeresScanMatcher2D() {}

/**
 * @brief 基于Ceres的扫描匹配
 * 
 * @param[in] target_translation 预测出来的先验位置, 只有xy
 * @param[in] initial_pose_estimate (校正后的)先验位姿, 有xy与theta
 * @param[in] point_cloud 用于匹配的点云 点云的原点位于local坐标系原点
 * @param[in] grid 用于匹配的栅格地图
 * @param[out] pose_estimate 优化之后的位姿
 * @param[out] summary 
 */
void CeresScanMatcher2D::Match(const Eigen::Vector2d& target_translation,
                               const transform::Rigid2d& initial_pose_estimate,
                               const sensor::PointCloud& point_cloud,
                               const Grid2D& grid,
                               transform::Rigid2d* const pose_estimate,
                               ceres::Solver::Summary* const summary) const {
  double ceres_pose_estimate[3] = {initial_pose_estimate.translation().x(),
                                   initial_pose_estimate.translation().y(),
                                   initial_pose_estimate.rotation().angle()};
  ceres::Problem problem;

  double occupied_space_weight = 1.0;
  double translation_weight = 10.0;
  double rotation_weight = 40.0;

  // 地图部分的残差
  CHECK_GT(occupied_space_weight, 0.0);
  switch (grid.GetGridType()) {
    case GridType::PROBABILITY_GRID:
      problem.AddResidualBlock(
          CreateOccupiedSpaceCostFunction2D(
              occupied_space_weight /
                  std::sqrt(static_cast<double>(point_cloud.size())),
              point_cloud, grid),
          nullptr /* loss function */, ceres_pose_estimate);
      // problem.AddResidualBlock(
      //     CreateOccupiedGridCostFunction2D(
      //         occupied_space_weight /
      //             std::sqrt(static_cast<double>(point_cloud.size())),
      //         point_cloud, initial_pose_estimate, grid),
      //     nullptr /* loss function */, ceres_pose_estimate);
      break;
    case GridType::TSDF:
      problem.AddResidualBlock(
          CreateTSDFMatchCostFunction2D(
              occupied_space_weight /
                  std::sqrt(static_cast<double>(point_cloud.size())),
              point_cloud, static_cast<const TSDF2D&>(grid)),
          nullptr /* loss function */, ceres_pose_estimate);
      break;
  }

  // LOG(INFO) << " translation_weight ===== : " << translation_weight << std::endl;
  
  // 平移的残差
  CHECK_GT(translation_weight, 0.);
  problem.AddResidualBlock(
      TranslationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          translation_weight, target_translation), // 平移的目标值, 没有使用校准后的平移
      nullptr /* loss function */, ceres_pose_estimate);      // 平移的初值

  // 旋转的残差, 固定了角度不变
  CHECK_GT(rotation_weight, 0.0);
  problem.AddResidualBlock(
      RotationDeltaCostFunctor2D::CreateAutoDiffCostFunction(
          rotation_weight, ceres_pose_estimate[2]), // 角度的目标值
      nullptr /* loss function */, ceres_pose_estimate);       // 角度的初值

  // 根据配置进行求解
  ceres::Solve(ceres_solver_options_, &problem, summary);

  *pose_estimate = transform::Rigid2d(
      {ceres_pose_estimate[0], ceres_pose_estimate[1]}, ceres_pose_estimate[2]);

  LOG(INFO) << "ceres scan matcher : \n" << summary->BriefReport() << std::endl;
  // LOG(INFO) << summary->FullReport() << std::endl;

}

}  // namespace scan_matching
}  // namespace cartographer
